﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Windows;
using System.Windows.Controls;
using System.Windows.Data;
using System.Windows.Documents;
using System.Windows.Input;
using System.Windows.Media;
using System.Windows.Media.Imaging;
using System.Windows.Navigation;
using System.Windows.Shapes;

using Microsoft.Kinect;
using System.IO;
using System.Media;


namespace KinectDepthCam
{
    /// <summary>
    /// Interaction logic for MainWindow.xaml
    /// </summary>
    public partial class MainWindow : Window
    {
        KinectSensor myKinect;

        public MainWindow()
        {
            InitializeComponent();
        }

        private void Window_Loaded(object sender, RoutedEventArgs e)
        {
            if (KinectSensor.KinectSensors.Count == 0)
            {
                MessageBox.Show("No Kinects detected", "Camera Viewer");
                Application.Current.Shutdown();
            }

            try
            {
                myKinect = KinectSensor.KinectSensors[0];

                myKinect.DepthStream.Enable();

                myKinect.Start();

                myKinect.DepthFrameReady += new EventHandler<DepthImageFrameReadyEventArgs>(myKinect_DepthFrameReady);
            }
            catch
            {
                MessageBox.Show("Kinect initialise failed", "Depth Viewer");
                Application.Current.Shutdown();
            }
        }


        #region Depth code

        const int slotSizeFactor = 7;
        const int noOfSlots = 4096 >> slotSizeFactor;

        int[] SlotCounts = new int[noOfSlots];

        byte[] depthColorImage = null;

        void setColorImage(short[] depthData, int width, int height)
        {

            // Clear the depth counters in each slot
            for (int slotNo = 0; slotNo < SlotCounts.Length; slotNo++)
            {
                SlotCounts[slotNo] = 0;
            }

            // Make a pass through the image counting depth pixels

            for (int depthPos = 0; depthPos < depthData.Length; depthPos++)
            {
                int depthValue = depthData[depthPos] >> 3;
                // Check for the invalid values

                if (depthValue == myKinect.DepthStream.UnknownDepth ||
                    depthValue == myKinect.DepthStream.TooFarDepth ||
                    depthValue == myKinect.DepthStream.TooNearDepth)
                {
                    continue;
                }

                // Got a valid depth value
                // Find the slot and increase the counter

                int slotNumber = depthValue >> slotSizeFactor;
                SlotCounts[slotNumber]++;
            }

            // Now find the depth with the most pixels
            int maxCount = 0;
            int maxPos = 0;

            for (int slotNo = 0; slotNo < SlotCounts.Length; slotNo++)
            {
                if (SlotCounts[slotNo] > maxCount)
                {
                    maxCount = SlotCounts[slotNo];
                    maxPos = slotNo;
                }
            }

            // Now color those pixels red in the image

            int depthColorImagePos = 0;

            for (int depthPos = 0; depthPos < depthData.Length; depthPos++)
            {
                int depthValue = depthData[depthPos] >> 3;
                // Check for the invalid values

                if (depthValue == myKinect.DepthStream.UnknownDepth ||
                    depthValue == myKinect.DepthStream.TooFarDepth ||
                    depthValue == myKinect.DepthStream.TooNearDepth)
                {
                    // Color invalid pixels black
                    // Blue
                    depthColorImage[depthColorImagePos] = 0;
                    depthColorImagePos++;
                    // Green
                    depthColorImage[depthColorImagePos] = 0;
                    depthColorImagePos++;
                    // Red
                    depthColorImage[depthColorImagePos] = 0;
                    depthColorImagePos++;
                }
                else
                {
                    byte depthByte = (byte)(255 - (depthValue >> 4));

                    if (depthValue >> slotSizeFactor == maxPos)
                    {
                        // Blue
                        depthColorImage[depthColorImagePos] = 0;
                        depthColorImagePos++;
                        // Green
                        depthColorImage[depthColorImagePos] = 0;
                        depthColorImagePos++;
                        // Red
                        depthColorImage[depthColorImagePos] = depthByte;
                        depthColorImagePos++;
                    }
                    else
                    {
                        // Blue
                        depthColorImage[depthColorImagePos] = depthByte;
                        depthColorImagePos++;
                        // Green
                        depthColorImage[depthColorImagePos] = depthByte;
                        depthColorImagePos++;
                        // Red
                        depthColorImage[depthColorImagePos] = depthByte;
                        depthColorImagePos++;
                    }
                }
                // transparency
                depthColorImagePos++;
            }
        }

        #endregion


        short[] depthData = null;

        WriteableBitmap depthImageBitmap = null;

        void myKinect_DepthFrameReady(object sender, DepthImageFrameReadyEventArgs e)
        {
            using (DepthImageFrame depthFrame = e.OpenDepthImageFrame())
            {
                if (depthFrame == null) return;

                if (depthData == null)
                    depthData = new short[depthFrame.PixelDataLength];

                if (depthColorImage == null)
                    depthColorImage = new byte[depthFrame.PixelDataLength * 4];

                depthFrame.CopyPixelDataTo(depthData);

                setColorImage(depthData, depthFrame.Width, depthFrame.Height);

                // we now have a new array of color data

                if (depthImageBitmap == null)
                {
                    this.depthImageBitmap = new WriteableBitmap(
                        depthFrame.Width,
                        depthFrame.Height,
                        96,  // DpiX
                        96,  // DpiY
                        PixelFormats.Bgr32,
                        null);
                    kinectDepthImage.Source = depthImageBitmap;
                }

                this.depthImageBitmap.WritePixels(
                    new Int32Rect(0, 0, depthFrame.Width, depthFrame.Height),
                    depthColorImage, // video data
                    depthFrame.Width * 4, // stride,
                    0   // offset into the array - start at 0
                    );
            }
        }

        private void Dump_Click(object sender, RoutedEventArgs e)
        {
            StreamWriter s = new StreamWriter(@"dump.csv");
            for (int i = 1; i < SlotCounts.Length; i++)
            {
                s.Write(SlotCounts[i] + ",");
            }
            s.Close();
        }
    }
}
